from coppeliasim_zmqremoteapi_client import RemoteAPIClient
import ImpedenceControllerJoint
import math
import numpy as np

client = RemoteAPIClient()
sim = client.require('sim')
sim.stopSimulation()
print('成功与仿真软件建立通信！', flush=True)



Joint01 = sim.getObject('/Joint01')
Joint02 = sim.getObject('/Joint02')

previous_speed_J1 = 0
previous_speed_J2 = 0


result = []
for third in range(10, -39, -1):
    fourth = -30 - third
    row = [third, fourth]
    result.append(row)

result = np.array(result)
result = np.radians(result)



try:
    sim.startSimulation()
    while True:

        for point_index, point in enumerate(result):
            # 设置关节目标位置
            for i in range(10):

                target_angle_J1 = point[0]  # 目标关节角度
                target_angle_J2 = point[1]
                print('目标关节角度111:', target_angle_J1, target_angle_J2, flush=True)

                print("当前是第", flush=True)
                print(point_index, flush=True)
                print("个点", flush=True)

                target_angle_J1 = target_angle_J1% (2 * math.pi)
                target_angle_J2 = target_angle_J2% (2 * math.pi)
                if target_angle_J1 > math.pi:
                    target_angle_J1 = target_angle_J1 - 2 * math.pi
                if target_angle_J2 > math.pi:
                    target_angle_J2 = target_angle_J2 - 2 * math.pi

                print('目标关节角度:', target_angle_J1, target_angle_J2, flush=True)

                # 获取当前关节角度
                current_angle_J1 = sim.getJointPosition(Joint01)
                current_angle_J2 = sim.getJointPosition(Joint02)
                # 获取当前关节速度
                current_velocity_J1 = sim.getJointVelocity(Joint01)
                current_velocity_J2 = sim.getJointVelocity(Joint02)
                if current_velocity_J1 > previous_speed_J1:
                    previous_speed_J1 = current_velocity_J1
                if current_velocity_J2 > previous_speed_J2:
                    previous_speed_J2 = current_velocity_J2
                print("当前最大角速度", previous_speed_J1, previous_speed_J2, flush=True)


                # 计算关节力矩
                force_J1, force_J2 = ImpedenceControllerJoint.ImpedanceController( target_angle_J1, target_angle_J2, current_angle_J1, current_angle_J2, current_velocity_J1, current_velocity_J2,D=1, K=1)

                sim.setJointTargetForce(Joint01, force_J1)
                sim.setJointTargetForce(Joint02, force_J2)
                if force_J1 >0:
                    sim.setJointTargetVelocity(Joint01, 10)
                else:
                    sim.setJointTargetVelocity(Joint01, -10)

                if force_J2 >0:
                    sim.setJointTargetVelocity(Joint02, 10)
                else:
                    sim.setJointTargetVelocity(Joint02, -10)

finally:
    sim.stopSimulation()
    print('仿真结束！', flush=True)